#include "loc_correction/filter_utils.h"


MovingAverageFilter::MovingAverageFilter(int window_size) : window_size(window_size) {}

Eigen::Vector3d MovingAverageFilter::apply(const Eigen::Vector3d& new_position) {
  
    if (position_queue.size() >= window_size) {
        position_queue.pop();
    }
    position_queue.push(new_position);
    
    Eigen::Vector3d sum = Eigen::Vector3d::Zero();
    std::queue<Eigen::Vector3d> temp_queue = position_queue;
    while (!temp_queue.empty()) {
        sum += temp_queue.front();
        temp_queue.pop();
    }
    
    return sum / position_queue.size();
}

